\documentclass[a4paper,12pt,fleqn]{article}
\usepackage{amsmath}
\usepackage{graphicx}


\usepackage{amsmath}
\usepackage{amssymb}
\usepackage{subfigure}
\usepackage{amssymb}
\usepackage{bm}

\newcommand{\nin}{n_u}
\newcommand{\nx}{n_x}

\newcommand{\lQP}{l}
\newcommand{\uQP}{u}
\newcommand{\AQP}{\mathbf{A}}
\newcommand{\PQP}{\mathbf{P}}
\newcommand{\qQP}{\mathbf{p}}
\newcommand{\xQP}{\mathbf{z}}

\newcommand{\MPC}{\mathrm{MPC}}
\newcommand{\varx}{{X}}
\newcommand{\varu}{{U}}
\newcommand{\varepsvec}{\bm{\epsilon}}
\newcommand{\varxvec}{\mathbf{x}}
\newcommand{\varuvec}{\mathbf{u}}
\newcommand{\slack}{\epsilon}
\newcommand{\QxN}{Q_{x_N}}
\newcommand{\Qx}{Q_{x}}
\newcommand{\Qu}{Q_{u}}
\newcommand{\Qdu}{Q_{\Delta u}}
\newcommand{\Np}{{N_p}}
\newcommand{\Nc}{{N_c}}
\newcommand{\blkdiag}{\text{blkdiag}}
\newcommand{\kron}{\text{kron}}
\newcommand{\baru}{u_{-1}}

\begin{document}

 \title{pyMPC Documentation}
\author{Marco Forgione}

\maketitle

\section{Mathematical formulation}

\subsection{Dynamical system}
We consider a linear, discrete-time dynamical system with $\nin$ inputs and $\nx$ states:
\begin{align}
 x_{k+1} = A x_{k} + B u_{k},
\end{align}
with $A \in \mathbb{R}^{\nx \times \nx}$ and $B \in \mathbb{R}^{\nx \times \nin}$.

\subsection{MPC cost function}
The Model Predictive Control (MPC) problem solved by pyMPC is:
\begin{subequations}
\label{eq:MPC}
\begin{align}
  &\arg \min_{\varu} 
  \overbrace{\frac{1}{2}\big(x_N - x_{\rm ref}\big)^\top \QxN \big(x_N - x_{\rm ref}\big)}^{=J_{Q_{x_N}}} + 
  \overbrace{\frac{1}{2}\sum_{k=0}^{\Np-1} \big(x_k - x_{\rm ref}\big)^\top \Qx\big(x_k - x_{\rm ref}\big)}^{J_{Q_x}}+ \nonumber \\
  &  + 
    \overbrace{\frac{1}{2}\sum_{k=0}^{\Np-1} \big(u_k - u_{\rm ref}\big)^\top \Qu \big(u_k - u_{\rm ref}\big)}^{J_u}
    +  
  \overbrace{\frac{1}{2}\sum_{k=0}^{\Np-1} \Delta u_k^\top \Qdu \Delta u_k}^{J_{\Delta u}} \\ \nonumber
  &\text{subject to}: \nonumber\\
  &x_{k+1} = Ax_k + B u_k \label{eq:linear_dynamics} \\ 
  &u_{\rm min} \leq u_k \leq u_{\rm max}\\
  &x_{\rm min} \leq x_k \leq x_{\rm max}\\
  &\Delta u_{\rm min} \leq \Delta u_k \leq \Delta u_{\rm max}\\
  &x_0 = \bar x\\
  &u_{-1} = \bar u,
\end{align}
\end{subequations} where $\Delta u_k = u_k - u_{k-1}$ and the optimization variables are the elements of the input sequence:
\begin{align}
  \varu & = \left\{ u_0,\ u_1,\ \dots, \ u_{\Np-1} \right \}.
%\begin{bmatrix}u_0 & u_1 & \dots & u_{\Np-1}\end{bmatrix}.
\end{align}

According to the numerical solution strategy, the elements of the state sequence:
\begin{equation}
  \varx  = \left\{ x_0,\ x_1,\ \dots, \ x_{\Np} \right \}\\ 
\end{equation}
may be either included as optimization variables of the Quadratic Programming (QP) problem (in the sparse formulation), or eliminated (in the condensed formulation). The current implementation of pyMPC is based on the sparse formulation and utilizes the QP solver OSQP. 

\subsection{Notation}
Bolt symbols are used to denote column vectors containing the stacked elements of quantities of interest over the prediction horizon:
\begin{subequations}
\begin{align}
 \varuvec^\top &= \begin{bmatrix} u_0^\top &u_1^\top &\dots& u_{\Np-1}^\top\end{bmatrix}^\top,\\
 \varxvec^\top &= \begin{bmatrix} x_0^\top &x_1^\top &\dots& x_{\Np}^\top\end{bmatrix}^\top,\\
 {\varxvec}_{\rm ref}^\top &= \begin{bmatrix} x_{\rm ref}^\top &x_{\rm ref}^\top &\dots& x_{\rm ref}^\top\end{bmatrix}^\top,\\
 {\varuvec}_{\rm ref}^\top &= \begin{bmatrix} u_{\rm ref}^\top &u_{\rm ref}^\top &\dots& u_{\rm ref}^\top\end{bmatrix}^\top,\\
 {\varxvec}_{\rm min}^\top &= \begin{bmatrix} x_{\rm min}^\top &x_{\rm min}^\top &\dots& x_{\rm min}^\top\end{bmatrix}^\top,\\
 {\varxvec}_{\rm max}^\top &= \begin{bmatrix} x_{\rm max}^\top &x_{\rm max}^\top &\dots& x_{\rm max}^\top\end{bmatrix}^\top,\\
 {\varuvec}_{\rm min}^\top &= \begin{bmatrix} u_{\rm min}^\top &u_{\rm min}^\top &\dots& u_{\rm min}^\top\end{bmatrix}^\top,\\
 {\varuvec}_{\rm max}^\top &= \begin{bmatrix} u_{\rm max}^\top &u_{\rm max}^\top &\dots& u_{\rm max}^\top\end{bmatrix}^\top,\\
 \Delta{\varuvec}_{\rm min}^\top &= \begin{bmatrix} \Delta u_{\rm min}^\top &\Delta u_{\rm min}^\top &\dots& \Delta u_{\rm min}^\top\end{bmatrix}^\top,\\
 \Delta{\varuvec}_{\rm max}^\top &= \begin{bmatrix} \Delta u_{\rm max}^\top &\Delta u_{\rm max}^\top &\dots& \Delta u_{\rm max}^\top\end{bmatrix}^\top.
 \end{align}
\end{subequations}
Note that we consider constant references and constraints over the prediction horizon for notation simplicity. The extension to time-varying references and constraints is straightforward.
 
\subsection{Receding horizon implementation}
In a typical implementation, the MPC input is applied in \emph{receding horizon}. At each time step $i$, problem \eqref{eq:MPC} is solved with $x_0=x[i],\;u_{-1}=u[{i-1}]$ and an optimal input sequence $u_{0},\dots,u_{\Np}$ is obtained. The first element of this sequence $u_0$ is the control input that is actually applied at time instant $i$. At time instant $i+1$, a new state $x[i+1]$ is measured (or estimated), and the process is iterated. 

Thus, formally, the MPC control law is a (static) function of the current state and the previous input:
\begin{equation}
 u_{MPC} = K(x[i], u[i-1]).
\end{equation}

Note that this function also depends on the references $x_{\rm ref}$ and $u_{\rm ref}$ and on the system matrices $A$ and $B$.

\section{Quadratic Programming Formulation}
%The OSQP Quadratic Programming (QP) solver expects a problem with form: 
Available Quadratic Programming (QP) solvers such as OSQP and QPOases expect a problem having a standard form\footnote{The exact  formulation is solver-dependent, but the different formulations are substantially equivalent.} such as:
\begin{subequations}
\label{eq:QP}
\begin{align}
 &\min \frac{1}{2} \xQP^\top \PQP \xQP +  \qQP^\top \xQP \\
 &\text{subject to} \nonumber \\
 &\lQP \leq \AQP \xQP \leq \uQP.
\end{align}
\end{subequations}
Thus, to implement the MPC controller using a standard QP solver, we need to re-write the MPC optimization problem \eqref{eq:MPC} in form
\eqref{eq:QP}. 

\section{Sparse QP Formulation}
\label{sec:sparse}
In the \emph{sparse} QP formulation of the MPC problem, the decision variables to be optimized are both $\varuvec$ and $\varxvec$. The dynamic constraints given by the model equations \eqref{eq:linear_dynamics} are included explicitly as equality constraints on the state variables 
$\varxvec$.

The resulting QP problem is high-dimensional, sparse, and highly structured. 
Certain QP solvers can take advantage of sparsity and structure of the QP problem and perform well with this formulation, despite its high dimensionality.
%. Thus, in certain cases, the advantages of sparsity and structure 
%, despite its high dimensionality.

\subsection{Cost function}
\subsubsection{Terms in $Q_x$ and $Q_{x_N}$}
By direct inspection, the non-constant terms of the cost function in $Q_x$ and $Q_{x_N}$ are:
\begin{multline}
\label{eq:J_Qx}
 J_{Q_x} = \frac{1}{2}
 \begin{bmatrix}
  x_0^\top & x_1^\top &\dots & x_{\Np}^\top
 \end{bmatrix}^\top
 \overbrace{\blkdiag(Q_x, Q_x, \dots, Q_{x_N})}^{=\mathcal{Q}_x}
 \begin{bmatrix}
  x_0 \\  x_1\\ \vdots\\  x_{\Np}
 \end{bmatrix} + \\
 +
 \overbrace{
  \begin{bmatrix}
  -{x_{\rm ref}}^\top Q_x & -{x_{\rm ref}}^\top Q_x &\dots & -{x_{\rm ref}}^\top Q_{x_N}
 \end{bmatrix} 
 }^{p_{Q_x}}
 \begin{bmatrix}
  x_0 \\ x_1 \\ \vdots \\ x_{\Np}
 \end{bmatrix}^\top.
 \end{multline}
 More compactly, this is equivalent to:
 \begin{equation}
  J_{Q_x} = \frac{1}{2} \varxvec^\top \mathcal{Q}_x \varxvec - \varxvec_{\rm ref}^\top \mathcal{Q}_x \varxvec = \frac{1}{2} \varxvec^\top \mathcal{Q}_x \varxvec + p_{Q_x}^\top \varxvec.
 \end{equation}

 \subsubsection{Terms in $Q_u$}
 Similarly, for the term $J_{Q_u}$: 
\begin{multline}
\label{eq:J_Qu}
 J_{Q_u} = \frac{1}{2}
 \begin{bmatrix}
  u_0^\top & u_1^\top &\dots & u_{\Np-1}^\top
 \end{bmatrix}
 \overbrace{\blkdiag(Q_u, Q_u, \dots, Q_u)}^{\mathcal{Q}_u}
 \begin{bmatrix}
  u_0 \\  u_1\\ \vdots\\  u_{\Np-1}
 \end{bmatrix}
  + \\
 +
 \overbrace{
  \begin{bmatrix}
  -{u_{\rm ref}}^\top Q_u & -{u_{\rm ref}}^\top Q_u &\dots & -{u_{\rm ref}}^\top Q_u
 \end{bmatrix}
 }^{p_{Q_u}}
 \begin{bmatrix}
  u_0 \\ u_1 \\ \vdots \\ u_{\Np-1}
 \end{bmatrix} 
 \end{multline}
 More compactly, this is equivalent to:
 \begin{equation}
  J_{Q_u} = \frac{1}{2} \varuvec^\top \mathcal{Q}_u \varuvec - \varuvec_{\rm ref}^\top \mathcal{Q}_u
 \varuvec = \frac{1}{2} \varuvec^\top \mathcal{Q}_u \varuvec + p_{Q_u}
 \varuvec 
 .
 \end{equation}

  \subsubsection{Terms in $Q_{\Delta u}$}
 As for the terms in $Q_{\Delta u}$, we have instead:
 \begin{small}
\begin{multline}
 J_{\Delta u} = \frac{1}{2}
 \begin{bmatrix}
  u_0 & u_1 &\dots & u_{\Np-1}
 \end{bmatrix}^\top
 \overbrace{
  \begin{bmatrix}
  2Q_{\Delta u} & -Q_{\Delta u} &0                  & \dots         & \dots  &   0\\
  -Q_{\Delta u} & 2Q_{\Delta u} &-Q_{\Delta u}      &0              & \dots  &   0\\
  0             & -Q_{\Delta u} &\ddots             & \ddots        &\ddots  &   0\\
  0             & 0             &\ddots             & \ddots        &\ddots  &   0\\  
  0             & 0             &\ddots             & \ddots        &\ddots  &   0\\
  0             & 0             &0                  &-Q_{\Delta u}  & 2Q_{\Delta u} &-Q_{\Delta u}\\  
  0             & 0             &0                  &0              & -Q_{\Delta u} &Q_{\Delta u}\\  
  \end{bmatrix}
  }^{\mathcal{Q}_{\Delta u}}
 \begin{bmatrix}
  u_0 \\  u_1\\ \vdots\\  u_{\Np-1}
 \end{bmatrix}^\top \\
-\baru^\top Q_{\Delta u}u_0
 \end{multline} 
 \end{small}
 More compactly, this is equivalent to:
\begin{equation*}
 J_{\Delta u} = \frac{1}{2} \varuvec^\top \mathcal{Q}_{\Delta u} \varuvec -\baru^\top Q_{\Delta u}u_0
\end{equation*}
 It is convenient to write the above expression using stacked vectors only:
\begin{equation}
 J_{\Delta u} = \frac{1}{2} \varuvec^\top \mathcal{Q}_{\Delta u} \varuvec +   
 \overbrace{
 \begin{bmatrix}
  -{\baru}^\top Q_{\Delta u} & 0 & \dots  & 0
 \end{bmatrix}}^{p_{Q_{\Delta u}}}
 \varuvec
\end{equation}
 
\subsection{Constraints}
\subsubsection{Linear dynamics}
Let us consider the linear equality constraints \eqref{eq:linear_dynamics} representing the system dynamics. These can 
be written in matrix form as:
\begin{small}
\begin{equation}
\begin{bmatrix}
x_0\\
x_1\\
\vdots\\
x_{\Np-1}\\
x_{\Np}
\end{bmatrix}=
\overbrace{
\begin{bmatrix}
 0      &0      &\dots  &0\\
 A_d    &0      &\dots  & 0\\
 0      &A_d    &\dots  &0\\
 \vdots &0      &\ddots & 0\\
 0      &0      &\dots  &A_d
\end{bmatrix}
}^{=\mathcal{A}}
\begin{bmatrix}
x_0\\
x_1\\
\vdots\\
x_{\Np-1}\\
x_{\Np} 
\end{bmatrix} +
\overbrace{
\begin{bmatrix}
 0      &0      &\dots  &0\\
 B_d    &0      &\dots  & 0\\
 0      &B_d    &\dots  &0\\
 \vdots &0      &\ddots & 0\\
 0      &0      &\dots  &B_d
\end{bmatrix}
}^{=\mathcal{B}}
\begin{bmatrix}
u_0\\
u_1\\
\vdots\\
u_{\Np-2}\\
u_{\Np-1} 
\end{bmatrix} +
\overbrace{
\begin{bmatrix}
\bar x\\
0\\
\vdots\\
\vdots \\
0 
\end{bmatrix}
}^{=\mathcal{C}}.
\end{equation}
\end{small}
Thus, we get a set of linear equality constraints representing the system dynamics \eqref{eq:linear_dynamics}.
These constraints can be written as
\begin{equation}
 \begin{bmatrix}
  (\mathcal{A}-I) & \mathcal{B}
 \end{bmatrix}
 \begin{bmatrix}
  \varxvec\\
  \varuvec
 \end{bmatrix}
 = -\mathcal{C}.
\end{equation}
\subsubsection{Variable bounds: $x$ and $u$}
The bounds on $x$ and $u$ are readily implemented as:
\begin{equation}
\begin{bmatrix}
 \varxvec_{\rm min}\\\varuvec_{\rm min}
\end{bmatrix}
\leq
\begin{bmatrix}
 I &0\\
 0 & I
\end{bmatrix}
\begin{bmatrix}
 \varxvec\\ \varuvec
\end{bmatrix}
\leq
\begin{bmatrix}
 \varxvec_{\rm max}\\ \varuvec_{\rm max}
\end{bmatrix}.
\end{equation}
\subsubsection{Input increment bound: $\Delta u$}
The input increment bound for all the time steps may be written as:
\begin{equation}
\begin{bmatrix}
u_{-1} +\Delta u_{\rm min}\\
\Delta u_{\rm min}\\
\vdots\\
\Delta u_{\rm min}\\
\end{bmatrix} \leq 
\overbrace{
\begin{bmatrix}
  I  &  0 & \dots & \dots  & 0 & 0\\
 -I  &  I &  0    & \dots  & 0 & 0\\
  0  & -I &  I    & \dots  & 0 & 0\\
	\vdots\\
	0  &  0 & \dots & 0      &-I & I\\    
\end{bmatrix}
}^{={\Delta}}
\begin{bmatrix}
u_0\\
u_1\\
\vdots\\
u_{\Nc-1}\\
\end{bmatrix}
\leq 
\begin{bmatrix}
u_{-1} +\Delta u_{\rm max}\\
\Delta u_{\rm max}\\
\vdots\\
\Delta u_{\rm max},\\
\end{bmatrix}
\end{equation}
where we have defined the matrix $\Delta$ performing the finite difference operation on $\varuvec$.

\subsection{Soft constraints}
Bounds on $x$ may result in an unfeasible QP problem! A common solution
is to transform the hard constraints in $x$ into soft constraints by means of  \emph{slack variables} $\varepsvec$. 
In the current implementation, there are as many slack variables as state variables, i.e. $\varepsvec \in \mathbb{R}^{\Np\nx \times 1}$.
We use the constraint:
\begin{equation}
\begin{bmatrix}
 x_{\rm min}\\u_{\rm min}
\end{bmatrix}
\leq
\begin{bmatrix}
 I &0 &I\\
 0 &I & 0
\end{bmatrix}
\begin{bmatrix}
 \varxvec\\
 \varuvec\\
 \varepsvec
\end{bmatrix}
\leq
\begin{bmatrix}
 x_{\rm max}\\u_{\rm max}
\end{bmatrix}.
\end{equation}

In order to penalize constraint violation, we have a penalty term in the cost function:
\begin{equation}
 J_{\slack} = \frac{1}{2} \varepsvec^\top \mathcal{Q}_{\slack} \varepsvec,
\end{equation}
where 
\begin{equation}
\mathcal{Q}_{\slack} = \blkdiag(Q_{\slack}, Q_{\slack}, \dots Q_{\slack})
\end{equation}
and $Q_{\slack}$ is $\sigma I_{\nx}$, with $\sigma$ a ``large'' constant (e.g. $\sigma=10^4$).

\subsection{Control Horizon}
Sometimes, we may want to use a control horizon $\Nc < \Np$ instead of the standard $\Nc = \Np$. In this case, the input considered is constant for $\Nc \geq \Np$.

\begin{equation}
\begin{bmatrix}
x_0\\
x_1\\
\vdots\\
x_{\Np-1}\\
x_{\Np}
\end{bmatrix}=
\overbrace{
\begin{bmatrix}
 0      &0      &\dots  &0\\
 A_d    &0      &\dots  & 0\\
 0      &A_d    &\dots  &0\\
 \vdots &0      &\ddots & 0\\
 0      &0      &\dots  &A_d
\end{bmatrix}
}^{=\mathcal{A}}
\begin{bmatrix}
x_0\\
x_1\\
\vdots\\
x_{\Np-1}\\
x_{\Np} 
\end{bmatrix} +
\overbrace{
\begin{bmatrix}
 0      &0      &\dots  &0\\
 B_d    &0      &\dots  & 0\\
 0      &B_d    &\dots  &0\\
 \vdots &0      &\ddots & 0\\
 0      &0      &\dots  &B_d\\
 0      &0      &\dots  &\vdots\\
 0      &0      &\dots  &B_d\\
\end{bmatrix}
}^{=\mathbb B}
\begin{bmatrix}
u_0\\
u_1\\
u_2\\
\vdots\\
u_{\Nc-1}\\
\vdots\\
u_{\Nc-1}
\end{bmatrix} +
\overbrace{
\begin{bmatrix}
\bar x\\
0\\
\vdots\\
\vdots \\
0 
\end{bmatrix}
}^{=\mathcal{C}}
\end{equation}

The contributions $J_{Q_u}$ of the cost function also changes:
\begin{multline}
\label{eq:J_Qu_Nc}
 J_{Q_u} = \frac{1}{2}
 \begin{bmatrix}
  u_0^\top & u_1^\top &\dots & u_{\Np-1}^\top
 \end{bmatrix}
 \blkdiag\bigg(Q_u, Q_u, \dots, (\Np - \Nc + 1)Q_u\bigg)
 \begin{bmatrix}
  u_0 \\  u_1\\ \vdots\\  u_{\Np-1}
 \end{bmatrix}
  + \\
 +
  \begin{bmatrix}
  -{u_{\rm ref}}^\top Q_u & -{u_{\rm ref}}^\top Q_u &\dots & -(\Np - \Nc + 1){u_{\rm ref}}^\top Q_u
 \end{bmatrix}
 \begin{bmatrix}
  u_0 \\ u_1 \\ \vdots \\ u_{\Np-1}
 \end{bmatrix}
 \end{multline}
Instead, $J_\Delta u$ does not change (because the input is constant for $k \geq N_c$!

\subsection{Overall formulation}
The overall sparse QP formulation is thus%may %OSQP Quadratic Programming (QP) solver expects a problem with form: 
\begin{subequations}
%\label{eq:QP}
\begin{align}
 &\min \frac{1}{2} \xQP^\top \PQP \xQP +  \qQP^\top \xQP \\
 &\text{subject to} \nonumber \\
 &\lQP \leq \AQP \xQP \leq \uQP,
\end{align}
\end{subequations}
where 
\begin{subequations}
\begin{align}
\xQP^\top &= \begin{bmatrix}\varxvec^\top \; \varuvec^\top\; \varepsvec^\top \end{bmatrix}\\
\PQP &= \blkdiag(\mathcal{Q}_x, \mathcal{Q}_u + \mathcal{Q}_{\Delta u}, \mathcal{Q}_\slack) \\
\qQP^\top &= \begin{bmatrix}p_{Q_x}^\top, p_{Q_u}^\top + p_{\Delta_u}^\top, 0 \end{bmatrix} \label{pQPsparse} \\
\AQP  &=  
 \begin{bmatrix}
  (\mathcal{A}-I) & \mathcal{B} & 0 \\
  I & 0 & I\\
  0 & I & 0\\
  0 & \Delta & 0 \label{eq:AQPsparse}\\
 \end{bmatrix}\\
\lQP^\top &= \begin{bmatrix}-\mathcal{C}^\top, \varxvec_{\min}^\top, \varuvec_{\min}^\top, \Delta{\varuvec}_{\rm min}^\top \end{bmatrix}^\top \label{lQPsparse}\\
\uQP^\top &= \begin{bmatrix}-\mathcal{C}^\top, \varxvec_{\max}^\top, \varuvec_{\max}^\top, \Delta{\varuvec}_{\rm max}^\top \end{bmatrix}^\top \label{uQPsparse}.
\end{align}
\end{subequations}

\subsection{Implementation details}
\paragraph{Exploiting sparsity}
The sparse QP formulation of MPC may be convenient when it is used in combination with a QP solver that is able to 
exploit sparsity and structure of the problem. The OSQP solver seems to be a good candidate. 
The sparse QP matrices have to be implemented in a sparse format (e.g., using the
\texttt{scipy.sparse} API in Python).
\paragraph{Minimal update of the QP matrices}
The terms $\mathcal{C}$ in \eqref{lQPsparse}, \eqref{uQPsparse} and $p_{\Delta u}$ in \eqref{pQPsparse} have to be modified at each iterationas as they depend on the initial state $x_0$ and the previously
applied input $u_{-1}$, respectively. In the case of time-varying system matrices $A_d$ and $B_d$, the terms $\mathcal{A}$ and 
$\mathcal{B}$ in \eqref{eq:AQPsparse} also change and have to be updated. All the other terms do not change for the different iterations
of the MPC. It is important to perform a minimal on-line update of the system matrices to save computational time.
\paragraph{Kronecker product}
Several matrices appearing in the expressions above may be more easily implemented using the \emph{kronecker product}.
For instance, the $\mathcal{A}$ is equivalent to:
\begin{equation}
\mathcal{A} = \kron \left (
 \begin{bmatrix}
 0      &0      &\dots  &0\\
 1      &0      &\dots  & 0\\
 0      &1     &\dots  &0\\
 \vdots &0      &\ddots & 0\\
 0      &0      &\dots  &1
\end{bmatrix}, A_d \right)
\end{equation}
In Python code, we obtain the sparse matrix $\mathcal{A}$ with the syntax:
\begin{verbatim}
 A_cal = sparse.kron(sparse.eye(N, k=-1), Ad).
\end{verbatim}
Similarly, we would obtain the dense version of $\mathcal{A}$ with the syntax:
\begin{verbatim}
 A_cal = np.kron(np.eye(N, k=-1), Ad).
\end{verbatim}


\section{Condensed QP formulation}
In the \emph{condensed} QP formulation, the input sequence vector $\varuvec$ are the only optimization variables. The state variables are eliminated by applying the so-called Lagrange equations:
\begin{equation}
\begin{bmatrix}
x_1\\
x_2\\
\vdots\\
x_{\Np-1}\\
x_{\Np}
\end{bmatrix}=
\overbrace{
\begin{bmatrix}
A_d\\
A_d^2\\
\vdots\\
A_d^{\Np-1}\\
A_d^\Np
\end{bmatrix}
}^{=\mathcal{A}}
x_0 + 
\overbrace{
\begin{bmatrix}
 B_d                &0              &0              &\dots      &0      &0\\
 A_dB_d               &B_d            &0              &\dots      &0      &0\\
 \dots              &\dots          &\dots          &\dots     &0       &0\\
 A_d^{\Np-2}B_d       &A_d^{\Np-3}B_d   &A_d^{\Np-4}B_d   &\dots     &B_d       &0\\
 A_d^{\Np-1}B_d       &A_d^{\Np-2}B_d   &A_d^{\Np-3}B_d   &\dots     &A_dB_d      &B_d \\
\end{bmatrix}
}^{=\mathcal B}
\begin{bmatrix}
u_0\\
u_1\\
u_2\\
\vdots\\
u_{\Np-1}
\end{bmatrix}.
\end{equation}
In vector notation, this is simply:
\begin{equation}
 \varxvec = \mathcal{A}x_0 + \mathcal{B} \varuvec
\end{equation}
The resulting QP problem is lower-dimensional than the one obtained with the sparse formulation (Section \ref{sec:sparse}), as $\varxvec$ is no longer a free decision variable. However, sparsity and structural properties of the resulting QP problem are generally lost. 
%For certain MPC problems and QP solvers, the benefit of having a smaller 
%Certain QP solvers are more efficient 
%when used in combination with a 

\subsection{Cost function}
The elements of the cost function may be written compactly as:
\begin{equation}
\label{eq:JQx_dense}
 J_{Q_x} = \frac{1}{2} \left(\mathcal{A} x_0 + \mathcal{B} \varuvec - \varxvec_{\rm ref}\right)^\top \mathcal{Q}_x \left(\mathcal{A} x_0 + \mathcal{B} \varuvec - \varxvec_{\rm ref}\right)
\end{equation}

\begin{equation}
 J_{Q_u} =  \frac{1}{2} (\varuvec - \varuvec_{\rm ref})^\top \mathcal{Q}_u (\varuvec-\varuvec_{\rm ref})
\end{equation}

\begin{equation}
 J_{Q_{\Delta u}} =  \frac{1}{2} \varuvec^\top \mathcal{Q}_{\Delta u} \varuvec +   
 \begin{bmatrix}
  -{\baru}^\top Q_{\Delta u} & 0 & \dots  & 0
 \end{bmatrix} \varuvec
\end{equation}

Summing up and expanding terms:
\begin{multline}
 J = 
 \frac{1}{2}\varuvec^\top \mathcal{B}^\top \mathcal{Q}_x \mathcal{B} \varuvec + 
 \frac{1}{2} (\mathcal{A}x_0 - \varxvec_{\rm ref})^\top \mathcal{Q}_x (\mathcal{A}x_0-\varxvec_{\rm ref}) +
 (\mathcal{A}x_0 -\varxvec_{\rm ref})^\top\mathcal{Q}_x \mathcal{B} \varuvec + \\
 + \frac{1}{2} \varuvec^\top \mathcal{Q}_u \varuvec +
 \frac{1}{2}{\varuvec_{\rm ref}}^\top \mathcal{Q}_u \varuvec_{\rm ref} + 
 -{\varuvec_{\rm ref}}^\top\mathcal{Q}_u \varuvec +\\ 
 +\frac{1}{2} \varuvec^\top \mathcal{Q}_{\Delta u} \varuvec +
\begin{bmatrix}
  -{\baru}^\top Q_{\Delta u} & 0 & \dots  & 0
 \end{bmatrix} \varuvec 
\end{multline}
Neglecting constant terms and collecting:
\begin{multline}
 J = C + \frac{1}{2} \varuvec^\top \overbrace{\big(\mathcal{B}^\top \mathcal{Q}_x \mathcal{B} + \mathcal{Q}_u + \mathcal{Q}_{\Delta u}\big)} ^{=\PQP}\varuvec + \\
 \overbrace{\bigg [(\mathcal{A}x_o - \varxvec_{\rm ref})^\top \mathcal{Q}_x \mathcal{B} -{\varuvec_{\rm ref}}^\top\mathcal{Q}_u -
 \begin{bmatrix}
  {\baru}^\top Q_{\Delta u} & 0 & \dots  & 0
 \end{bmatrix}\bigg)}^{=\qQP^\top} \varuvec
\end{multline}
Thus, we have
\begin{equation}
 \qQP = \mathcal{B}^\top \mathcal{Q}_x(\mathcal{A}x_0 - \varxvec_{\rm ref}) - \mathcal{Q}_u\varuvec_{\rm ref} + 
  \begin{bmatrix}
 -Q_{\Delta u} \baru \\ 0 \\ \vdots \\0
 \end{bmatrix}
\end{equation}
expanding
\begin{equation}
 \qQP = \overbrace{\mathcal{B}^\top \mathcal{Q}_x\mathcal{A}}^{=p_{x_0}}x_0  \;+\; \overbrace{-\mathcal{B}^\top \mathcal{Q}_x}^{p_{\varxvec_{\rm ref}}}\varxvec_{\rm ref}\;+\; \overbrace{-\mathcal{Q}_u}^{=p_{\varuvec_{\rm ref}}}\varuvec_{\rm ref} + 
\overbrace{
  \begin{bmatrix}
 -Q_{\Delta u} \\ 0 \\ \vdots \\0
 \end{bmatrix}
}^{=p_{\baru}}
\baru
\end{equation}

\subsection{Control horizon}
In this case, we define:
\begin{equation}
\varuvec^\top = \begin{bmatrix} u_0^\top &u_1^\top &\dots& u_{\Nc-1}^\top\end{bmatrix}^\top.
\end{equation}

We can exploit the equation:
\begin{equation}
 \begin{bmatrix}
  u_0 \\ u_1 \\ u_2 \\ \vdots\\ u_{\Np-1}
 \end{bmatrix} = 
 \overbrace{
 \begin{bmatrix}
 I      &0      &\dots  & 0\\
 0      &I      &\dots  &0\\
 \vdots &0      &\ddots & 0\\
 0      &0      &\dots  &I\\
 0      &0      &\dots  &\vdots\\
 0      &0      &\dots  &I\\
\end{bmatrix}
}^{=S}
 \begin{bmatrix}
  u_0 \\ u_1 \\ u_2 \\ \vdots\\ u_{\Nc-1}
 \end{bmatrix} 
 = S \varuvec
\end{equation}
Then, the state variables satisfy:
\begin{equation}
 \varxvec = \mathcal{A}x_0 + \mathcal{B}S \varuvec.
\end{equation}
Then, the cost function term $J_{Q_x}$ can then be computed as in \eqref{eq:JQx_dense}, by re-defining $\mathcal{B} = S\mathcal{B}$.
The term $J_{Q_u}$ should also be adjusted by re-defining $\mathcal{Q}_u = \blkdiag\left (Q_u, Q_u, \dots (\Np - \Nc + 1)Q_u \right)$, as the last element of the input sequence in the control horizon has to be weighted for $(\Np - \Nc + 1)$ time steps.
%[And also adjusting the term $J_{Q_u}$, if present...]

\subsection{Implementation details}
By adopting the condensed MPC formulation, we obtain a lower-dimensional QP problem as the state variables are eliminated. 
However, the sparsity properties of the resulting QP problem are generally lost. In particular, the matrix $\PQP$ is in general 
fully populated. 

In practice, we expect good performance from the condensed formulation for relatively small MPC problems, and using solvers optimized 
for dense QP problems. A good candidate may be QPOases.

\subsection{The unconstrained case}
By dropping the constraints on $u$ $\Delta u$, and $x$, the condensed formulation allows writing the solution of the MPC problem
in closed-form. In fact, for an unconstrained QP problem, the optimal value of $\varuvec$ is:
\begin{equation}
 \label{eq:QPclosedform}
 \varuvec^{\rm opt} = -\PQP^{-1}\qQP.
\end{equation}
Expanding, we obtain in our case:
\begin{equation}
 \varuvec^{\rm opt} = k_{x_0} x_0 + k_{\varxvec_{\rm ref}}\varxvec_{\rm ref} + k_{\varuvec_{\rm ref}} \varuvec_{\rm ref} k_{\baru} \baru
\end{equation}
with:
\begin{subequations}
\begin{align}
\label{eq:LMPC}
k_{x_0} &= -\PQP^{-1}p_{x_0}\\
k_{\varxvec_{\rm ref}} &= -\PQP^{-1}p_{\varxvec_{\rm ref}}\\
k_{\varuvec_{\rm ref}} &= -\PQP^{-1}p_{\varuvec_{\rm ref}}\\
k_{\baru} &= -\PQP^{-1}p_{\baru}.
\end{align}
\end{subequations}
Thus, the control law is a simple linear function of $x_0$, $\varxvec_{\rm ref}$, $\varuvec_{\rm ref}$, and $u_{-1}$.


For constant system matrices $A$ and $B$, the QP matrices $\PQP$ and $\qQP$ are also fixed. It is then convenient to compute the inverse of $\PQP$ off-line and use formulas $\eqref{eq:LMPC}$ for real-time computations. In the case of time-varying system matrices, the QP matrices change at each iteration of MPC. Then, it is perhaps more efficient and numerically stable to obtain the solution of \eqref{eq:QPclosedform} by using standard routines for solving linear systems, instead of computing the inverse of $\PQP$ on-line.
\end{document} 

